#coding=utf-8
'''
    三维旋转矩阵实现
'''
import numpy as np
from definations.definations import ERROR
np.set_printoptions(precision=2)
np.set_printoptions(suppress=True)
def Trans3D(inMat=np.zeros((1,3)),r=0.0,p=0.0,y=0.0,trans=np.zeros((3,1))):
    '''
        实现点坐标的3D旋转
    :param inMat:
    :param r:
    :param p:
    :param y:
    :return:
    '''
    rMat=np.array([[1.0, 0.0, 0.0],
                   [0.0, np.cos(r),-np.sin(r)],
                  [0.0, np.sin(r),np.cos(r)]])
    pMat=np.array([[np.cos(p), 0.0, np.sin(p)],
                   [0.0, 1.0, 0.0],
                   [-np.sin(p),0.0,np.cos(p)]])
    yMat=np.array([[np.cos(y), -np.sin(y), 0.0],
                   [np.sin(y),np.cos(y), 0.0],
                   [0.0, 0.0, 1.0]])
    #   执行旋转变换
    retMat=(rMat@(pMat@(yMat@inMat)))
    #   执行平移变换
    return retMat+trans


def D_H_Trans3D(d=0,theta=0,a=0,alpha=0):

    T = np.array([
        [np.cos(theta), -np.sin(theta) * np.cos(alpha), np.sin(theta) * np.sin(alpha), a * np.cos(theta)],
        [np.sin(theta), np.cos(theta) * np.cos(alpha), -np.cos(theta) * np.sin(alpha), a * np.sin(theta)],
        [0.0, np.sin(alpha), np.cos(alpha), d],
        [0.0, 0.0, 0.0, 1.0]
    ])
    return T

def ExtractRPFromDH(DH):
    '''
    从D-H矩阵中提取旋转-位置信息
    :param DH:
    :return:
    '''
    if np.shape(DH)!=(4,4):
        raise Exception(ERROR.DATALEN_INCOMPATABLE)
    R=DH[0:3,0:3]
    P=DH[0:3,-1].transpose()
    return R,P

if __name__=='__main__':
    J1=D_H_Trans3D(0,0,0,np.pi/2)
    J2=D_H_Trans3D(1,0,0,np.pi/2)
    J3=D_H_Trans3D(1,0,0,0)
    J4 = D_H_Trans3D(0, 0, 0, 0)
    print(J1@J2@J3@J4)